/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.orbits;

import ore.Frame;
import ore.PositionVelocity;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Vector3D;

/**
 * The parameters used internally are the circular elements which can be
 * related to keplerian elements as follows:
 *   <ul>
 *     <li>a</li>
 *     <li>e<sub>x</sub> = e cos(&omega;)</li>
 *     <li>e<sub>y</sub> = e sin(&omega;)</li>
 *     <li>i</li>
 *     <li>&Omega;</li>
 *     <li>&alpha;<sub>v</sub> = v + &omega;</li>
 *   </ul>
 * where &Omega; stands for the Right Ascension of the Ascending Node and
 * &alpha;<sub>v</sub> stands for the true longitude argument
 * 
 * The conversion equations from and to keplerian elements given above hold only
 * when both sides are unambiguously defined, i.e. when orbit is neither equatorial
 * nor circular. When orbit is circular (but not equatorial), the circular
 * parameters are still unambiguously defined whereas some keplerian elements
 * (more precisely &omega; and &Omega;) become ambiguous. When orbit is equatorial,
 * neither the keplerian nor the circular parameters can be defined unambiguously.
 * {@link EquinoctialOrbit equinoctial orbits} is the recommended way to represent
 * orbits.
 * 
 * An instance of this class is immutable.
 * 
 * @see    ore.Orbit
 * @see    KeplerianOrbit
 * @see    CartesianOrbit
 * @see    EquinoctialOrbit
 * 
 * @author Luc Maisonobe
 * @author Fabien Maussion
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class CircularOrbit
    extends ore.Orbit
{

    public static final int MEAN_LONGITUDE_ARGUMENT = 0;
    public static final int ECCENTRIC_LONGITUDE_ARGUMENT = 1;
    public static final int TRUE_LONGITUDE_ARGUMENT = 2;


    /** 
     * Semi-major axis (m).
     */
    private final double a;
    /**
     * First component of the circular eccentricity vector.
     */
    private final double ex;
    /** 
     * Second component of the circular eccentricity vector.
     */
    private final double ey;
    /** 
     * Inclination (rad).
     */
    private final double i;
    /** 
     * Right Ascension of Ascending Node (rad).
     */
    private final double raan;
    /** 
     * True longitude argument (rad).
     */
    private final double alphaV;


    /** 
     * @param a  semi-major axis (m)
     * @param ex e cos(&omega;), first component of circular eccentricity vector
     * @param ey e sin(&omega;), second component of circular eccentricity vector
     * @param i inclination (rad)
     * @param raan right ascension of ascending node (&Omega;, rad)
     * @param alpha  an + &omega;, mean, eccentric or true longitude argument (rad)
     * @param type type of longitude argument, must be one of {@link #MEAN_LONGITUDE_ARGUMENT},
     * {@link #ECCENTRIC_LONGITUDE_ARGUMENT} or  {@link #TRUE_LONGITUDE_ARGUMENT}
     * @param frame the frame in which are defined the parameters
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     * @exception IllegalArgumentException if the longitude argument type is not
     * one of {@link #MEAN_LONGITUDE_ARGUMENT}, {@link #ECCENTRIC_LONGITUDE_ARGUMENT}
     * or {@link #TRUE_LONGITUDE_ARGUMENT}
     * @see #MEAN_LONGITUDE_ARGUMENT
     * @see #ECCENTRIC_LONGITUDE_ARGUMENT
     * @see #TRUE_LONGITUDE_ARGUMENT
     */
    public CircularOrbit(double a, double ex, double ey,
                         double i, double raan,
                         double alpha, int type,
                         Frame frame, AbsoluteDate date, double mu)
        throws IllegalArgumentException
    {
        super(frame, date, mu);
        this.a    =  a;
        this.ex   = ex;
        this.ey   = ey;
        this.i    = i;
        this.raan = raan;

        switch (type) {
        case MEAN_LONGITUDE_ARGUMENT :
            this.alphaV = this.computeAlphaM(alpha);
            break;
        case ECCENTRIC_LONGITUDE_ARGUMENT :
            this.alphaV = this.computeAlphaE(alpha);
            break;
        case TRUE_LONGITUDE_ARGUMENT :
            this.alphaV = alpha;
            break;
        default :
            throw OrekitException.createIllegalArgumentException(
                  "angle type not supported, supported angles: {0}, {1} and {2}",
                  "MEAN_LONGITUDE_ARGUMENT", "ECCENTRIC_LONGITUDE_ARGUMENT",
                  "TRUE_LONGITUDE_ARGUMENT");
        }

    }
    /** 
     * Cartesian parameters.
     * @param pvCoordinates the {@link PositionVelocity} in inertial frame
     * @param frame the frame in which are defined the {@link PositionVelocity}
     * @param date date of the orbital parameters
     * @param mu central attraction coefficient (m<sup>3</sup>/s<sup>2</sup>)
     */
    public CircularOrbit(PositionVelocity pvCoordinates, Frame frame,
                         AbsoluteDate date, double mu)
    {
        super(pvCoordinates, frame, date, mu);
        /*
         * Compute semi-major axis
         */
        Vector3D pvP = pvCoordinates.getPosition();
        Vector3D pvV = pvCoordinates.getVelocity();
        double r  = pvP.getNorm();
        double V2 = Vector3D.dotProduct(pvV, pvV);
        double rV2OnMu = ((r * V2) / mu);
        this.a = (r / (2 - rV2OnMu));
        /*
         * Compute inclination
         */
        Vector3D momentum = Vector3D.crossProduct(pvP, pvV);
        this.i = Vector3D.angle(momentum, Vector3D.PLUS_K);
        /*
         * Compute right ascension of ascending node
         */
        Vector3D node     = Vector3D.crossProduct(Vector3D.PLUS_K, momentum);
        double   n2       = Vector3D.dotProduct(node, node);
        /*
         * The following comparison with 0 IS REALLY numerically justified and stable
         */
        this.raan = (n2 == 0) ? 0 : Math.atan2(node.getY(), node.getX());
        /*
         * 2D-coordinates in the canonical frame
         */
        double cosRaan = Math.cos(this.raan);
        double sinRaan = Math.sin(this.raan);
        double cosI    = Math.cos(this.i);
        double sinI    = Math.sin(this.i);
        Vector3D rVec  = new Vector3D(cosRaan, sinRaan, 0);
        Vector3D sVec  = new Vector3D(-cosI * sinRaan, cosI * cosRaan, sinI);
        double x2      = Vector3D.dotProduct(pvP, rVec) / this.a;
        double y2      = Vector3D.dotProduct(pvP, sVec) / this.a;
        /*
         * Compute eccentricity vector
         */
        double eSE    = Vector3D.dotProduct(pvP, pvV) / Math.sqrt(mu * this.a);
        double eCE    = (rV2OnMu - 1);
        double e2     = ((eCE * eCE) + (eSE * eSE));
        double f      = (eCE - e2);
        double g      = Math.sqrt(1 - e2) * eSE;
        double aOnR   = (this.a / r);
        double a2OnR2 = (aOnR * aOnR);
        this.ex = (a2OnR2 * (f * x2 + g * y2));
        this.ey = (a2OnR2 * (f * y2 - g * x2));
        /*
         * Compute longitude argument
         */
        double beta = (1.0 / (1.0 + Math.sqrt(1.0 - this.ex * this.ex - this.ey * this.ey)));
        this.alphaV = this.computeAlphaE(Math.atan2((y2 + this.ey + eSE * beta * this.ex),
                                                    (x2 + this.ex - eSE * beta * this.ey)));
    }
    /** 
     * @param op Orbital parameters to copy
     */
    public CircularOrbit(ore.Orbit op) {
        super(op);
        this.a    = op.getA();
        this.i    = op.getI();
        this.raan = Math.atan2(op.getHy(), op.getHx());
        double cosRaan = Math.cos(this.raan);
        double sinRaan = Math.sin(this.raan);
        double equiEx = op.getEquinoctialEx();
        double equiEy = op.getEquinoctialEy();
        this.ex   = ((equiEx * cosRaan) + (equiEy * sinRaan));
        this.ey   = ((equiEy * cosRaan) - (equiEx * sinRaan));
        this.alphaV = (op.getLv() - this.raan);
    }


    /** 
     * @return Semi-major axis (m)
     */
    public double getA() {
        return this.a;
    }
    /** 
     * @return e cos(&omega; + &Omega;), first component of the
     * eccentricity vector
     */
    public double getEquinoctialEx() {
        return ((this.ex * Math.cos(this.raan)) -
                (this.ey * Math.sin(this.raan)));
    }
    /** 
     * @return e sin(&omega; + &Omega;), second component of the
     * eccentricity vector
     */
    public double getEquinoctialEy() {
        return ((this.ey * Math.cos(this.raan)) +
                (this.ex * Math.sin(this.raan)));
    }
    /** 
     * @return ex = e cos(&omega;), first component of the circular
     * eccentricity vector
     */
    public double getCircularEx() {
        return this.ex;
    }
    /** 
     * @return ey = e sin(&omega;), second component of the circular
     * eccentricity vector
     */
    public double getCircularEy() {
        return this.ey;
    }
    /** 
     * @return First component of the inclination vector.
     */
    public double getHx() {
        return (Math.cos(this.raan) * Math.tan(this.i / 2));
    }
    /** 
     * @return second component of the inclination vector.
     */
    public double getHy() {
        return (Math.sin(this.raan) * Math.tan(this.i / 2));
    }
    /** 
     * @return Eccentricity
     */
    public double getE() {
        return Math.sqrt((this.ex * this.ex) + (this.ey * this.ey));
    }
    /** 
     * @return Inclination (rad)
     */
    public double getI() {
        return this.i;
    }
    /** 
     * @return Right ascension of the ascending node (rad)
     */
    public double getRightAscensionOfAscendingNode() {
        return this.raan;
    }
    /** 
     * @return true latitude argument (rad)
     */
    public double getLv() {
        return (this.alphaV + this.raan);
    }
    /** 
     * @return eccentric latitude argument.(rad)
     */
    public double getLE() {
        return (this.getAlphaE() + this.raan);
    }
    /**
     * @return Mean latitude argument.(rad)
     */
    public double getLM() {
        return (this.getAlphaM() + this.raan);
    }
    /** 
     * @return v + &omega; true longitude argument (rad)
     */
    public double getAlphaV() {
        return this.alphaV;
    }
    /** 
     * @return E + &omega; eccentric longitude argument (rad)
     */
    public double getAlphaE() {
        double epsilon   = Math.sqrt((1 - this.ex * this.ex) -
                                     (this.ey * this.ey));
        double cosAlphaV = Math.cos(this.alphaV);
        double sinAlphaV = Math.sin(this.alphaV);

        return (this.alphaV + 
                2.0 * Math.atan((this.ey * cosAlphaV - this.ex * sinAlphaV) /
                                (epsilon + 1 + this.ex * cosAlphaV + this.ey * sinAlphaV)));
    }
    /** 
     * @return M + &omega; mean longitude argument (rad)
     */
    public double getAlphaM() {
        double alphaE = this.getAlphaE();
        return (alphaE - 
                (this.ex * Math.sin(alphaE)) +
                (this.ey * Math.cos(alphaE)));
    }
    /** 
     * @param alphaE = E + &omega; eccentric longitude argument (rad)
     * @return the true longitude argument.
     */
    private double computeAlphaE(double alphaE) {
        double epsilon   = Math.sqrt((1 - this.ex * this.ex) -
                                     (this.ey * this.ey));
        double cosAlphaE = Math.cos(alphaE);
        double sinAlphaE = Math.sin(alphaE);

        return (alphaE + 
                2.0 * Math.atan((this.ex * sinAlphaE - this.ey * cosAlphaE) /
                                (epsilon + 1 - this.ex * cosAlphaE - this.ey * sinAlphaE)));
    }
    /** 
     * @param alphaM = M + &omega;  mean longitude argument (rad)
     * @return the true longitude argument.
     */
    private double computeAlphaM(double alphaM) {
        /*
         * Generalization of Kepler equation to equinoctial parameters
         * with alphaE = PA + E and
         *      alphaM = PA + M = alphaE - ex.sin(alphaE) + ey.cos(alphaE)
         */
        double alphaE = alphaM;
        double shift = 0.0;
        double alphaEMalphaM = 0.0;
        double cosLE = Math.cos(alphaE);
        double sinLE = Math.sin(alphaE);
        int iter = 0;
        do {
            double f2 = ((this.ex * sinLE) - (this.ey * cosLE));
            double f1 = (1.0 - (this.ex * cosLE) - (this.ey * sinLE));
            double f0 = (alphaEMalphaM - f2);

            double f12 = (2.0 * f1);
            shift = ((f0 * f12) / (f1 * f12 - f0 * f2));

            alphaEMalphaM -= shift;
            alphaE         = (alphaM + alphaEMalphaM);
            cosLE          = Math.cos(alphaE);
            sinLE          = Math.sin(alphaE);

        }
        while ((++iter < 50) && (Math.abs(shift) > 1.0e-12));

        return this.computeAlphaE(alphaE);
    }


    /**  Returns a string representation of this Orbit object.
     * @return a string representation of this object
     */
    public String toString() {
        return new StringBuffer().append("circular parameters: ").append('{').
                                  append("a: ").append(a).
                                  append(", ex: ").append(ex).append(", ey: ").append(ey).
                                  append(", i: ").append(i).
                                  append(", raan: ").append(raan).
                                  append(", alphaV: ").append(Math.toDegrees(alphaV)).
                                  append(";}").toString();
    }

}
